/* 运行逻辑					
信号输入			功率输出		
INAx	INBx	PWMx	Mx_A	Mx_B	电机状态
L	    L	    X	    L	    L	    制动
L	    H	    PWM	    PWM	    L	    正转
H	    L	    PWM	    L	    PWM  	反转
H   	H	    X   	Z   	Z   	悬空
驱动器发生过欠压保护或者过热保护			
Z	Z	悬空 */

#include "Motor.h"
//电机最大转速是500
//存储pid和motor的数据
PIDInstance MotorSpeedPID_Left;
PIDInstance MotorSpeedPID_Right;
MotorInstance Motor_data;
 
/** 
* @brief 初始化编码器
* @param none
* @return none  
*/
void Encoder_Init(void)
{	

	//初始化编码器
	HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);      //开启编码器定时器
    __HAL_TIM_ENABLE_IT(&htim3,TIM_IT_UPDATE);           //防溢出处理
	__HAL_TIM_SET_COUNTER(&htim3, 10000);                //初始值设定为10000

	// 启动定时器中断和基本计数功能
    HAL_TIM_Base_Start_IT(&htim3); // 编码器定时器
    HAL_TIM_Base_Start_IT(&htim4); // 10ms定时器

	
}

/** 
* @brief 初始化电机pwm，初始化电机数据
* @param none
* @return none
*/
void Motor_Init(void)
{
	//PWM 
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); // 开启 TIM2 通道3的 PWM
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); // 开启 TIM2 通道4的 PWM

	//初始化电机数据
	Motor_data.loopNum = 0; //溢出计数
    
}

/**
 * @brief 定时器中断回调函数
 * 
 * 根据不同的定时器实例处理对应的逻辑：
 * - 如果是 `htim4`，每 10ms 计算电机的总角度和速度。
 * - 如果是 `htim3`，处理编码器计数溢出情况。
 * 
 * @param htim 定时器句柄指针
 * 
 * @note 
 * - `htim4` 用于周期性计算电机速度。
 * - `htim3` 用于处理编码器计数溢出，确保角度计算正确。
 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == htim1.Instance) // 10ms中断，用于计算速度
    {
        int16_t pluse = COUNTERNUM - RELOADVALUE/2;
        //从开始到现在当前10ms的总脉冲数							               									
		Motor_data.totalAngle = pluse + Motor_data.loopNum * RELOADVALUE/2;
        //进行速度计算,根据前文所说的,4倍频,编码器13位,减速比30,再乘以6000即为每分钟输出轴多少转
        //motor.totalAngle - motor.lastAngle为当前10ms内的增量，即脉冲数
        Motor_data.speed_Right = (float)(Motor_data.totalAngle - Motor_data.lastAngle)/(4*13*RR)*6000;			
		Motor_data.lastAngle = Motor_data.totalAngle;              //更新转过的圈数
    }
    if (htim->Instance == htim4.Instance) 
    {
        int16_t pluse = COUNTERNUM - RELOADVALUE/2;							               									
		Motor_data.totalAngle = pluse + Motor_data.loopNum * RELOADVALUE/2;
        Motor_data.speed_Left = (float)(Motor_data.totalAngle - Motor_data.lastAngle)/(4*13*RR)*6000;			
		Motor_data.lastAngle = Motor_data.totalAngle;             
    }
    else if (htim->Instance == htim3.Instance) // 编码器溢出处理
    {
        if (COUNTERNUM < 10000)
            Motor_data.loopNum++; // 向上计数超过10000，正溢出+1
        else if (COUNTERNUM > 10000)
            Motor_data.loopNum--; // 向下计数小于0，负溢出+1
        __HAL_TIM_SET_COUNTER(&htim3, 10000); // 重新设定初始值
    }
}


/** 
* @brief 发送电机指令
* @param none
* @return none  
*/
void Motor_Send(void)
{
    
}

void motor_contol()
{
    
}
